#include "MyRobot.h"

/**
 * This code is run during the match after autonomous mode
 * is complete. This is where the majority of robot-running
 * code will occur. This file may get large when adding more
 * methods, and inherintly more complicated. Splitting this
 * page's methods into multiple pages will help keep the code
 * organized and easier to read, understand, debug, and update.
 */

void RobotDemo::OperatorControl()
{
	//Any initialization of stuff goes here, before the main
	//control loop begins.
	myRobot.SetSafetyEnabled(true);
	
	display.pauseDisplay();		//disables the display in match (saves resources from display thread)
	
	//PID
	bool start = false;
	
	//Gear Shift
	bool isInFirstGear = true;
	
	//Indexer
	bool isFeeding = false;
	bool lastSensorSaw = false;
	bool indexerWaiting = false;
	Timer indexTime;
	
	//Pyramid Lifter
	bool pyramidLocked = false;
	
	//Camera Feed
	bool lastTriggerPressed = false;
	
	while (IsOperatorControl() && IsEnabled())
	{
		myRobot.TankDrive(leftStick.GetY(), rightStick.GetY());//I love Jason
		/*
		//drive with tank drive (use leftStick and rightStick)
		if (leftStick.GetY() >= JOYSTICKDEADSPACE && rightStick.GetY() >= JOYSTICKDEADSPACE)
		{
			myRobot.TankDrive(leftStick.GetY(), rightStick.GetY());
		}
		else if (leftStick.GetY() <= JOYSTICKDEADSPACE && rightStick.GetY() >= JOYSTICKDEADSPACE)
		{
			myRobot.TankDrive(0.0, rightStick.GetY());
		}
		else if (leftStick.GetY() >= JOYSTICKDEADSPACE && rightStick.GetY() <= JOYSTICKDEADSPACE)
		{
			myRobot.TankDrive(leftStick.GetY(), 0.0);
		}
		else if (leftStick.GetY() <= JOYSTICKDEADSPACE && rightStick.GetY() <= JOYSTICKDEADSPACE)
		{
			myRobot.TankDrive(0.0, 0.0);
		}*/
		
		/*
		 * PID Control
		 */
		if (xbox.GetRawButton(XBOX_X) && !start)
		{
			PIDfirst.SetPID(RPMP, RPMI, RPMD);
			PIDfirst.SetSetpoint(PIDSET);
			PIDfirst.Enable();
			start = true;
			printf("Started PID (%lf, %lf, %lf), setpoint %lf\n", PIDfirst.GetP(), PIDfirst.GetI(), PIDfirst.GetD(), PIDfirst.GetSetpoint());
		}
		else if (!xbox.GetRawButton(XBOX_X))
		{
			if (start)
			{
				PIDfirst.Disable();
				printf("Stopped PID control.\n");
				start = false;
			}
			else
			{
				if (xbox.GetRawButton(XBOX_A))
				{
					shooterFirst.Set(-1.0);
					shooterSecond.Set(-1.0);
				}
				else
				{
					shooterFirst.Set(0.0);
					shooterSecond.Set(0.0);
				}
				//printf("First RPM: %lf\tSecond RPM: %lf\r", RPMGearToothFirst.PIDGet(), RPMGearToothSecond.PIDGet());
				//printf("WATCH:testificate,500");
			}
		}
		/*
		 * End PID control
		 */
		
		/*
		 * Gear Shifting
		 */
		if (rightStick.GetRawButton(2))
		{
			//FIRST GEAR
			if (!isInFirstGear)
			{
				driveShiftLeft.Set(0.0);
				driveShiftRight.Set(0.0);
				isInFirstGear = true;
			}
		}
		
		if (rightStick.GetRawButton(3))
		{
			//SECOND GEAR
			if (isInFirstGear)
			{
				driveShiftLeft.Set(1.0);
				driveShiftRight.Set(1.0);
				isInFirstGear = false;
			}
		}
		
		/*
		 * Pick-up
		 */
		if (xbox.GetRawButton(XBOX_B))
		{
			pickUp.Set(-1.0);
		}
		else
		{
			pickUp.Set(0.0);
		}
		/*
		 * End Pick-up
		 */
		
		/*
		 * Indexer
		 */
		if (xbox.GetRawButton(XBOX_START))
		{
			if (!isFeeding)
			{
				isFeeding = true;
				indexer.Set(INDEXERSPEED);
			}
		}
		
		if (isFeeding)
		{
			//if we are currently in the process of feeding
			//continue checking the sensor to see if it has reached the
			//mark it needs for a 1/3 rotation.
			if (xbox.GetRawButton(XBOX_BACK))
			{
				isFeeding = false;	//e-stop the feeder
			}
			
			if (indexerSensor.Get() == 1)
			{
				lastSensorSaw = false;
			}
			
			if (indexerSensor.Get() == 0 && !lastSensorSaw && !indexerWaiting)
			{	
				lastSensorSaw = true;
				indexerWaiting = true;
				indexTime.Start();
			}
			
			if (indexTime.HasPeriodPassed(INDEXERWAITPERIOD) && indexerWaiting)
			{
				isFeeding = false;
				indexerWaiting = false;
				indexer.Set(0.0);
				indexTime.Stop();
				indexTime.Reset();
			}
		}
		
		//reverse the indexer
		if (xbox.GetRawButton(XBOX_BACK))
		{
			isFeeding = false;	//stop indexing if was in the process already
			indexer.Set(-INDEXERSPEED / 2);	//go backwards slowly
		}
		else if (!isFeeding)
		{
			indexer.Set(0.0);	//stop indexer
		}
		/*
		 * End Indexer
		 */
		
		/*
		 * Loading Station Indexer (servo)
		 */
		if (xbox.GetRawAxis(XBOX_TRIGGERS) < 0)
		{
			loader.Set(1.0);
		}
		else if (xbox.GetRawAxis(XBOX_TRIGGERS) > 0)
		{
			loader.Set(0.0);
		}
		/*
		 * End Loading Station Indexer
		 */
		
		/*
		 * Pyramid Lifter
		 */
		if (xbox.GetRawButton(XBOX_LBUMP))
		{
			//only drive down when it's not at the bottom
			//if (pyramidIn.Get() == 1)
			//{
				//pyramidLifter.Set(0.0);
			//}
			//else
			//{
			    pyramidLifter.Set(-1.0);
			//}
		}
		else if (xbox.GetRawButton(XBOX_RBUMP))
		{
			//only drive up when it's not at the top
			//if (pyramidOut.Get() == 1)
			//{
				//pyramidLifter.Set(0.0);
			//}
			//else
			//{
				pyramidLifter.Set(1.0);
			//}
		}
		else
		{
			//only move when a button is pressed
			pyramidLifter.Set(0.0);
		}
		/*
		 * End Pyramid Lifter
		 */
		
		/*
		 * Change Camera Feed
		 */
		if (rightStick.GetRawButton(1))
		{
			if (!lastTriggerPressed)
			{
				//only change between separate presses of the trigger, not 
				//every cycle that the cRIO processes while it's held down
				displayShootFeed = !displayShootFeed;
				lastTriggerPressed = true;
			}
		}
		else 
		{
			lastTriggerPressed = false;
		}
		
		dispBat.setData(DriverStation::GetInstance()->GetBatteryVoltage());
		UpdateSmartDashboard();	//update the smart dashboard after everything is updated
		Wait(0.005); // wait for a motor update time
	}
	
	display.startDisplay(); //resume the display after the match
}
